#include <openrave-core.h>
#include <vector>
#include <sstream>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <boost/format.hpp>

#include <openrave/planningutils.h>

#include "BoxConfiguration.h"
#include "BaseCore.h"
#include "MyTrajectory.h"

using namespace OpenRAVE;
using namespace std;

typedef boost::shared_ptr<BoxConfiguration> BoxConfigurationPtr;

class TippingBox : public BaseCore
{
public:
	virtual void demothread(int argc, char ** argv)
	{
		string scenefilename = "data/tipping.env.xml";
		penv->Load(scenefilename);

		MyTrajectory* mt = new MyTrajectory("trajectory/chuan.csv");
		vector<vector<dReal>> waypoints;
		mt->CsvParser(waypoints);

		penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
		penv->GetPhysicsEngine()->SetGravity(Vector(0,-9.8,0));
		penv->SetCollisionChecker(RaveCreateCollisionChecker(penv, "ode"));

		vector<RobotBasePtr> vrobots;
		penv->GetRobots(vrobots);

		RobotBasePtr probot = vrobots.at(0);
		RobotBase::ManipulatorPtr pmanip = probot->GetManipulators().at(0);
		KinBodyPtr ptarget = penv->GetKinBody("magicbox");

		for(size_t i = 1; i < probot->GetManipulators().size(); ++i) {
			if( pmanip->GetArmIndices().size() < probot->GetManipulators()[i]->GetArmIndices().size() ) {
				pmanip = probot->GetManipulators()[i];
			}
		}
		RAVELOG_INFO(str(boost::format("planning with manipulator %s\n")%pmanip->GetName()));

		vector<vector<dReal>>::iterator iter = waypoints.begin();

		PlannerBasePtr planner = RaveCreatePlanner(penv,"birrt");
		probot->SetActiveDOFs(pmanip->GetArmIndices());

		while(IsOk()) 
		{
			EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

			PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
			params->_nMaxIterations = 4000; // max iterations before failure
			params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs
			params->vgoalconfig.resize(probot->GetActiveDOF());

			{
				RobotBase::RobotStateSaver saver(probot); // save the state
				std::copy((*iter).begin()+1, (*iter).end(), params->vgoalconfig.begin());
				params->vgoalconfig.back() -= 1.57;
				probot->SetActiveDOFValues(params->vgoalconfig);
			}

			RAVELOG_INFO("starting to plan\n");
			probot->GetActiveDOFValues(params->vinitialconfig);
			if( !planner->InitPlan(probot,params) ) {
				continue;
			}

			// create a new output trajectory
			TrajectoryBasePtr ptraj = RaveCreateTrajectory(penv,"");
			if( !planner->PlanPath(ptraj) ) {
				RAVELOG_WARN("plan failed, trying again\n");
				continue;
			}
			// send the trajectory to the robot
			probot->GetController()->SetPath(ptraj);

			break;
		}

		while(!probot->GetController()->IsDone() && IsOk()) {
			boost::this_thread::sleep(boost::posix_time::milliseconds(1));
		}

		std::vector<dReal> q;
		probot->GetActiveDOFValues(q); // get current values

		int timeStep = 0;

		TrajectoryBasePtr ptraj = RaveCreateTrajectory(penv,"");
		ConfigurationSpecification spec = probot->GetActiveConfigurationSpecification();
		ptraj->Init(spec);
		vector<dReal> vdata(spec.GetDOF(),0);

		iter = waypoints.begin();

		while(IsOk())
		{
			{
				EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
				while(iter < waypoints.end())
				{

					if( !!probot->GetController()) 
					{
						std::copy(q.begin(),q.end(),vdata.begin());
						ptraj->Insert(timeStep,vdata);

						std::copy((*iter).begin()+1, (*iter).end(), q.begin());
						q.back() -= 1.57;

						// check for collisions
						{
							RobotBase::RobotStateSaver saver(probot); // add a state saver so robot is not moved permenantly
							probot->SetActiveDOFValues(q);
							if(probot->CheckSelfCollision()) 
							{ // don't check env collisions since we have physics enabled
								continue; // robot in collision at final point, so reject
							}

						}
					}
					iter++;
					timeStep++;
				}

				if(iter == waypoints.end())
				{
					planningutils::SmoothActiveDOFTrajectory(ptraj,probot);
					probot->GetController()->SetPath(ptraj);
					iter++;
				}
			}
			if(penv->CheckCollision(probot, ptarget))
			{
				cout << "break" << endl;
			}
			while(!probot->GetController()->IsDone() && IsOk()) {
				boost::this_thread::sleep(boost::posix_time::milliseconds(1));
			}
		}
	}
};